﻿/// 计算移动很小距离的相机识别

#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

#include "CvCalibCommon.h"

#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

using namespace cv;
using namespace std;

static void doChessboardTinyCheck(Size boardSize, string inputFolder, string outputFolder,
                                  uint nCount, string fmtLeft, string fmtRight, int flags = IMREAD_GRAYSCALE )
{
    // 1. 读取前10个计算坐标
    Mat imgL, imgR;
    Mat imgGrayL, imgGrayR;
    vector<vector<Point2f>> cornersL(2*nCount);
    vector<vector<Point2f>> cornersR(2*nCount);
    char buf[30];
    int i;
    for (i = 0; i < 2*nCount; i++) {
        snprintf(buf, 30, fmtLeft.c_str(), i+1);
        imgL = imread(inputFolder + buf, flags);
        if (flags == IMREAD_COLOR) {
            cvtColor(imgL, imgL, COLOR_BGR2GRAY);
        }
        bool found1 = CvCalibCommon::findChessboardCorners(imgL, boardSize, cornersL[i]);
        if(!found1) {
            cout << "Not found corners in file " << buf << endl;
        }

        snprintf(buf, 30, fmtRight.c_str(), i+1);
        imgR = imread(inputFolder + buf, flags);
        if (flags == IMREAD_COLOR) {
            cvtColor(imgR, imgR, COLOR_BGR2GRAY);
        }
        bool found2 = CvCalibCommon::findChessboardCorners(imgR, boardSize, cornersR[i]);
        if(!found2) {
            cout << "Not found corners in file " << buf << endl;
        }
    }

    uint nPoint = boardSize.width * boardSize.height;
    // 2. 对结果进行统计处理
    vector<Point2f> means[4];
    for (i = 0;i< 4; i++) {
        means[i].resize(nPoint);
    }
    for (i = 0; i < nCount; i++) {
        cout << "cornerL " << i+1 << ",0: " << cornersL[i][0] << endl;
        cout << "cornerL2 " << i+1 << ",0: " << cornersL[nCount+i][0] << endl;
        cout << "cornerR " << i+1 << ",0: " << cornersR[i][0] << endl;
        cout << "cornerR2 " << i+1 << ",0: " << cornersR[nCount+i][0] << endl;
        for (int j = 0;j < 40; j++) {
            means[0][j] += cornersL[i][j];
            means[1][j] += cornersR[i][j];
            means[2][j] += cornersL[nCount+i][j];
            means[3][j] += cornersR[nCount+i][j];
        }
    }
    for (int j = 0;j < nPoint; j++) {
        for (i = 0; i<4; i++) {
            means[i][j] = means[i][j]/(int)nCount;
        }
    }

    for (int j = 0;j < nPoint; j++) {
        cout << j+1 << " L: " << means[0][j] - means[2][j];
        cout << " R: " << means[1][j] - means[3][j] << endl;
    }

//    cout << "L - 0: "<< means[0] << endl;
//    cout << "L - 1: "<< means[2] << endl;
//    cout << "R - 0: "<< means[1] << endl;
//    cout << "R - 1: "<< means[3] << endl;
}

static void doChessboardTinyCheck1(Size boardSize, string inputFolder, string outputFolder,
                                  uint nCount, string fmtImage, int flags = IMREAD_GRAYSCALE )
{
    // 1. 读取前10个计算坐标
    Mat imgL, imgR;
    Mat imgGrayL, imgGrayR;
    vector<vector<Point2f>> corners(2 * nCount);
    char buf[30];
    int i;
    for (i = 0; i < 2 * nCount; i++) {
        snprintf(buf, 30, fmtImage.c_str(), i+1);
        imgL = imread(inputFolder + buf, flags);
        if (flags == IMREAD_COLOR) {
            cvtColor(imgL, imgL, COLOR_BGR2GRAY);
        }
        bool found1 = CvCalibCommon::findChessboardCorners(imgL, boardSize, corners[i]);
        if(!found1) {
            cout << "Not found corners in file " << buf << endl;
        }
    }

    uint nPoint = boardSize.width * boardSize.height;
    // 2. 对结果进行统计处理
    vector<Point2f> means[2];
    for (i = 0;i< 2; i++) {
        means[i].resize(nPoint);
    }
    for (i = 0; i < nCount; i++) {
        cout << "corner1 " << i+1 << ",0: " << corners[i][0] << endl;
        cout << "corner2 " << i+1 << ",0: " << corners[nCount + i][0] << endl;
        for (int j = 0;j < 40; j++) {
            means[0][j] += corners[i][j];
            means[1][j] += corners[nCount+i][j];
        }
    }
    for (int j = 0;j < nPoint; j++) {
        for (i = 0; i<2; i++) {
            means[i][j] = means[i][j]/(int)nCount;
        }
    }

    for (int j = 0;j < nPoint; j++) {
        cout << j+1 << " L: " << means[0][j] - means[1][j] << endl;
    }

//    cout << "L - 0: "<< means[0] << endl;
//    cout << "L - 1: "<< means[2] << endl;
//    cout << "R - 0: "<< means[1] << endl;
//    cout << "R - 1: "<< means[3] << endl;
}

static void doBallFixedErr(string inputFolder, string outputFolder, int startIndex,
                           int nCount, int nThresh, String fmtImage, vector<Mat> &meanPoints)
{
    Mat img;
    vector<vector<Point2f>> corners(nCount);
    char buf[30];
    int i, j;
    for (i = 0; i <  nCount; i++) {
        snprintf(buf, 30, fmtImage.c_str(), i+1+startIndex);
        img = imread(inputFolder + buf, COLOR_BGR2GRAY);
        CvCalibCommon::findLightBalls(img, nThresh, corners[i]);
        cout << buf << " image found " << corners[i].size() << endl;
    }

    int nPoints = corners[0].size(); // boardSize.width  * boardSize.height;

    for (j=0;j<nPoints;j++) {
        cout << j+1 << ": ";
        Mat points = Mat::zeros(1, nCount, CV_64FC2);
        double *ptr = points.ptr<double>(0);
        for (i = 0; i <  nCount; i++) {
//            points.at<double>(0,i) = corners[i][j].x;
            *(ptr+i*2) = corners[i][j].x;
            *(ptr+i*2+1) = corners[i][j].y;
            // cout << "  " << corners[i][j];
        }
        cout << "  " << corners[0][j];

//        double minVal, maxVal;
//        minMaxLoc(points, &minVal, &maxVal);
//        Mat tempMean, tempStddv;
//        double means, stddev;
//        meanStdDev(points, tempMean, tempStddv);
//        means = tempMean.at<double>(0.0);
//        stddev = tempStddv.at<double>(0.0);
//        cout << " min: " << minVal <<", max: " << maxVal;
//        cout << " means: " << means <<", stddev: " << stddev;
//        cout << endl;


        double minVal[2], maxVal[2];
        //minMaxLoc(points, minVal, maxVal);

        Mat tempMean, tempStddv;
        //double means, stddev;
        meanStdDev(points, tempMean, tempStddv);
        meanPoints.push_back(tempMean);
        //means = tempMean.at<double>(0.0);
        //stddev = tempStddv.at<double>(0.0);
        //cout << "min: " << minVal[0] << ", " << minVal[1] <<", max: " << maxVal[0] << ", " << maxVal[1];
        cout << " means: " << tempMean.t() <<", stddev: " << tempStddv.t();
        cout << endl;
    }
}

static void demoTinyCheck()
{
    string inputFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/images10/";
    string outputFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/";

    Size boardSize = Size(8,5);

    doChessboardTinyCheck(boardSize, inputFolder, outputFolder, 10, "C0_%d.bmp", "C1_%d.bmp");


}

static void demoTinyCheckBS()
{
    string inputFolder = "D:/sai/opencv/images/bs/images10/";
    string outputFolder = "D:/sai/opencv/images/bs/";

    Size boardSize = Size(8,5);

    doChessboardTinyCheck1(boardSize, inputFolder, outputFolder, 3,  "C0_%d.jpg",
                          IMREAD_COLOR);

     uint nCount = 10;

}

static void demoFixedErr()
{
    string inputFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesfix2/";
    string outputFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/";
    Size boardSize = Size(8,5);

    int nCount = 10;

    String fmtImage = "C0_%d.bmp";
    Mat imgL, imgR;
    Mat imgGrayL, imgGrayR;
    vector<vector<Point2f>> corners(nCount);
    char buf[30];
    int i, j;
    for (i = 0; i <  nCount; i++) {
        snprintf(buf, 30, fmtImage.c_str(), i+1);
        imgL = imread(inputFolder + buf, COLOR_BGR2GRAY);
        bool found1 = CvCalibCommon::findChessboardCorners(imgL, boardSize, corners[i]);
        if(!found1) {
            cout << "Not found corners in file " << buf << endl;
        }
    }

    int nPoints = boardSize.width  * boardSize.height;


    for (j=0;j<nPoints;j++) {
        cout << j+1 << ": ";
        double maxd = 0;
        Mat points = Mat::zeros(1, nCount, CV_64FC2);
        double *ptr = points.ptr<double>(0);
        for (i = 0; i <  nCount; i++) {
            // points.at<double>(0,i) = corners[i][j].x;
            *(ptr+i*2) = corners[i][j].x;
            *(ptr+i*2+1) = corners[i][j].y;
            // cout << "  " << corners[i][j];
        }
        double minVal[2], maxVal[2];
        minMaxLoc(points, minVal, maxVal);
        Mat tempMean, tempStddv;
        //double means, stddev;
        meanStdDev(points, tempMean, tempStddv);
        //means = tempMean.at<double>(0.0);
        //stddev = tempStddv.at<double>(0.0);
        cout << "min: " << minVal[0] << ", " << minVal[1] <<", max: " << maxVal[0] << ", " << maxVal[1];
        cout << " means: " << tempMean.t() <<", stddev: " << tempStddv.t();
        cout << endl;
    }

}


static void demoBallFixedErr()
{
    string inputFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/imagesball2/";
    string outputFolder = "D:/sai/opencv/images/aimdata/chessboardEpson/";

    int nThresh = 90;
    int nCount = 10;

    String fmtImage = "C1_%d.bmp";

    vector<Mat> meanPts1, meansPts2;
    // 0-9
    doBallFixedErr(inputFolder, outputFolder, 0, nCount, nThresh, fmtImage, meanPts1);

    // 10-19
    doBallFixedErr(inputFolder, outputFolder, 10, nCount, nThresh, fmtImage, meansPts2);

    for (uint i=0; i<meanPts1.size();i++) {
        cout << i+1 << " : "<< norm(meanPts1[i], meansPts2[i], NORM_L2) << endl;
    }
}

int main(int argc, char *argv[])
{

    int option = 4;
    if (argc >= 2) {
        option = atoi(argv[1]);
    }

    switch (option) {
    case 1:
        demoTinyCheck();
        break;
    case 2:
        demoTinyCheckBS();
        break;
    case 3:
        demoFixedErr();
        break;
    case 4:
        demoBallFixedErr();
        break;
    default:
        cout << "Not support option: " << option << endl;
        break;
    }
    return 0;
}
